/*
 * Copyright (c) 2006-2025, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2025-09-29     Guotong    first version
 */

#include "servo.h"
#include <rtdbg.h>

#define DBG_TAG "servo"
#define DBG_LVL DBG_LOG

/* 全局变量 */
static int current_gear = 3;  // 初始档位为中间档位(70°，30-110度范围的中间位置)
static TIM_HandleTypeDef htim3;

/**
 * @brief  初始化舵机
 * @param  无
 * @retval 无
 */
void servo_init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    TIM_OC_InitTypeDef sConfigOC = {0};

    /* 启用TIM3时钟 */
    __HAL_RCC_TIM3_CLK_ENABLE();
    __HAL_RCC_GPIOA_CLK_ENABLE();

    /* 配置PWM输出引脚 */
    GPIO_InitStruct.Pin = SERVO_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(SERVO_PORT, &GPIO_InitStruct);

    /* 配置TIM3 */
    htim3.Instance = SERVO_TIM;
    htim3.Init.Prescaler = (SystemCoreClock / 1000000) - 1;  // 1MHz计数频率
    htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim3.Init.Period = SERVO_PERIOD - 1;  // 20ms周期
    htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    HAL_TIM_PWM_Init(&htim3);

    /* 配置PWM通道 */
    sConfigOC.OCMode = TIM_OCMODE_PWM1;
    sConfigOC.Pulse = SERVO_ANGLE_0 + (SERVO_ANGLE_180 - SERVO_ANGLE_0) * 90 / 180;  // 初始位置70度(30-110度范围的中间位置)
    sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
    sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
    HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, SERVO_CHANNEL);

    /* 启动PWM */
    HAL_TIM_PWM_Start(&htim3, SERVO_CHANNEL);
    
    LOG_D("Servo initialized on TIM3 CH1 (PA6)");
}

/**
 * @brief  设置舵机角度
 * @param  gear: 档位(0-6)
 * @retval 无
 */
void servo_set_gear(int gear)
{
    uint32_t pulse_width;
    int angle;

    /* 确保档位在有效范围内 */
    if (gear < 0) gear = 0;
    if (gear >= SERVO_GEAR_COUNT) gear = SERVO_GEAR_COUNT - 1;

    current_gear = gear;
    /* 计算对应档位的角度(30-110度范围) */
    angle = 30 + gear * 15;
    /* 计算对应档位的PWM脉宽 */
    pulse_width = SERVO_ANGLE_0 + (SERVO_ANGLE_180 - SERVO_ANGLE_0) * angle / 180;
    __HAL_TIM_SET_COMPARE(&htim3, SERVO_CHANNEL, pulse_width);
    LOG_D("Servo set to gear %d (angle: %d degrees)", gear, angle);
}

/**
 * @brief  获取当前舵机档位
 * @param  无
 * @retval 当前档位值(0-6)
 */
int servo_get_current_gear(void)
{
    return current_gear;
}
